'''
Created on Oct 11, 2013

@author: csiebern
'''

import numpy as np

def importCPTs(CPTfile):
    array = np.loadtxt(CPTfile,delimiter=',',skiprows=1)
    cpt = []
    for row in array:
        cpt.append(row)
    return cpt

class Sphere:
    """Least Squares Solution for fitting a calibration sphere with fixed radius using cartesian coordinate observations.
                            
    INPUTS:     center      - Sphere center estimate
                radius      - radius of calibration spheres in meters or "Free"
                clip        - Specify LiDAR clip ID to use
                sensor      - Specfify the LIDAR sensor
    OUTPUTS:    output_parm     - [1x4] numpy array of estimated Xc, Yc, Zc, r in meters
    """    
    def __init__(self, center, radius, clip, sensor):
        self.center   = center[1:4]
        self.radius   = radius
        self.clipid   = clip
        self.sensor   = sensor
        [self.points, self.clip_idx] = clip.getPointsNearXYZ(self.sensor,
                                                             self.center,
                                                             1.0,0.30)
        
    def fit(self, debug=False):
        x = self.points[0,:]
        y = self.points[1,:]
        z = self.points[2,:]       
        x = np.reshape(x,(len(x),1))
        y = np.reshape(y,(len(y),1))
        z = np.reshape(z,(len(z),1))
        R = self.radius
 
        # Assign the coordinate observations to l, an (nx1) matrix of coordinate observations
        l = np.vstack((x,y,z))
#         l = np.reshape(l,(len(l),1))
                   
        # Setup the general least squares model parameters
        """  3 points are required to define the center of a sphere using 9 coordinate observations. [3*3]
             because the radius is fixed 2 additional coordinate observations are required for each additional point [(n/3-3)*2]
        """
        n  = len(l)             # Total number of observations
        no = 3*3 + (n/3-3)*2    # Number of points required for an exact solution
        r  = n - no             # Degrees of freedom
        u  = 3                  # number of unknown parameters  
        c  = r + u              # Number of required condition equations
        
        # The model is nonlinear so an Initial Estimate of parameters is required  I will use the average 
        xc = self.center[0]
        yc = self.center[1]
        zc = self.center[2]        
        X  = np.array([[xc],[yc],[zc]])
        
        # perform the non-linear least squares adjustment using newton iterations        
        converged = False
        li = l      # initialize the observation update = current
        i  = 1      # initialize the iteration counter
        while converged == False:            
            # read parameter estimates for current iteration
            xc = X[0,0]
            yc = X[1,0]
            zc = X[2,0]
            
            # Calculate d, the distance from the point coordinates to the estimated center
            d = np.sqrt((x-xc)**2 + (y-yc)**2 + (z-zc)**2)
            d = np.reshape(d,(len(d),1))
            
            # Evaluate the first order linearization of observations A at current X
            a0 = (x - xc)/d
            a1 = (y - yc)/d
            a2 = (z - zc)/d
            A  = np.hstack((np.diag(a0.flat),np.diag(a1.flat),np.diag(a2.flat)))
                
            # Evaluate the jacobian B at current X
            b0 = (xc - x)/d
            b1 = (yc - y)/d
            b2 = (zc - z)/d
            B  = np.hstack((b0,b1,b2))
            
            # Evaluate f = -F(Xo)
            f = R - (d + np.dot(A,li-l))
            
            # Compute the Weight Matrix We from the cofactor matrix
            '''Assumed coordinate values are iid with variance 0.000036 m'''
            Q  = np.array([0.005**2,0.005**2,0.005**2]) #xyz observations are independent
            Q  = np.repeat(Q,c)
            Q = np.diag(Q)
            Qe = np.dot(A,np.dot(Q,A.T))
            We = np.linalg.inv(Qe) 
            
            # Solve for dX
            N = np.dot(B.T,np.dot(We,B))
            t =np.dot(B.T,np.dot(We,f))
            dX = np.dot(np.linalg.inv(N),t)        

            # Update parameter and calculate residuals
            X = X + dX
            k = np.dot(We,np.dot(-B,dX)+f)
            v = np.dot(np.dot(Q,A.T),k) 

            # Report dX and interation
            if debug==True:
                print i, dX[0], dX[1], dX[2]
                        
            # test for convergence
            if np.all(abs(dX) < 0.000001):
                converged      = True
                self.center    = X.flatten()
                self.residuals = np.reshape(v,(3,c))
                self.Qdd       = np.linalg.inv(N)
                
            else:
                if i > 30:
                    print 'Solution did not converge in 30 iterations, exiting'
                    return 
                # update observation vector for the next iteration 
                li = l + v
                i  = i + 1
        
    def clean(self,scale):
        eigval, eigvect = np.linalg.eigh(self.Qdd)
        axis = np.sqrt(eigval)*scale
        v    = np.dot(eigvect,self.residuals)
        
        vfilter = np.abs(v) < 0.005
        vfilter = np.any(vfilter, axis=0)
        self.points = self.points[:,vfilter]
        self.clip_idx = self.clip_idx[vfilter]
        